#!/usr/bin/python
# -*- coding: utf-8 -*-
"""
Analyse the result of a area scanning attempt
This script is for use independent of ROS!

READY FOR MIT LICENCE

man:
    analyse_are_scanning path/to/yaml/parameter/file.yaml  path/to/gps/log.cvs
"""

import csv
import sys
import yaml
from sailing_robot.navigation import Navigation
import numpy as np

# CSV values
# hhmmssdd
# Lat *10^^7
# Lon *10^^7


# Number of subdivision (8 for MS class)
sub = 6

# Load yaml file given in argument
yaml_file = sys.argv[1]
yaml_data = yaml.load(file(yaml_file, 'r'), Loader=yaml.Loader)

# gps log file given as the 2nd argument
cvs_file = sys.argv[2]


wpA = yaml_data['wp/table']['A']
wpB = yaml_data['wp/table']['B']


nav = Navigation()
wpA_utm = nav.latlon_to_utm(wpA[0], wpA[1])
wpB_utm = nav.latlon_to_utm(wpB[0], wpB[1])


position_utm = []
# Read log file
with open(cvs_file) as logfile:
    data = csv.reader(logfile, delimiter=',')
    for row in data:
        latitude = float(row[1])/10**7
        longitude = float(row[2])/10**7
        position_utm.append(nav.latlon_to_utm(latitude, longitude))



# Unit vector AB
vAB = np.array([wpB_utm[0] - wpA_utm[0], wpB_utm[1] - wpA_utm[1]])
AB = np.linalg.norm(vAB)
vAB = vAB/AB

# Unit vector orth to AB
vAB_orth = np.array([-vAB[1], vAB[0]])

output_array = np.zeros((sub, sub))
for point in position_utm:
    # projection of point in the (wpA_utm; vAB,vAB_orth) base
    point_base = [point[0] - wpA_utm[0], point[1] - wpA_utm[1]]
    point_i = np.dot(point_base, vAB)*sub/2/AB
    point_j = np.dot(point_base, vAB_orth)*sub/2/AB
    
    if point_i < 0 or point_j < 0 or point_i > sub or point_j > sub:
        continue 
    
    output_array[int(point_i)][int(point_j)] = 1


# Output the grid
print u'┌─' + u'┬─'*(sub-1) + u'┐'
score = 0
for i in range(sub):
    row = ''
    for j in range(sub):
        if output_array[i][j] == 1:
            if i<=(sub/2 -1) or j>(sub/2 -1):
                score += 1
                row = row + u"│X"
            else:
                row = row + u"│o"
        else:
            row = row + u"│ "
    print row + u'│'
    if i<(sub-1):
        print u'├─' + u'┼─'*(sub-1) + u'┤'
print u'└─' + u'┴─'*(sub-1) + u'┘'

print 'Score: ' +  str(score) + '/' + str(sub*sub*3/4)
